/** \file    bilinear.cpp
  * \brief   Bilinear interpolation of various image concerning objects.
  * \date    2001
  * \author  Fedor Chelnokov
  * \version $Header: /home/cscom1r4/cvsroot/cs/imglib/src/bilinear.cpp,v 1.2 2004/12/10 05:10:44 cscom1r4 Exp $
  */

#define _round_h_ //prevent inclusion of math/round.h

#include "imglib/bilinear.h"
#include "imglib/bitmap.h"
#include "math/float2int.h"
#include "xtd/arr2d_wise_iterator.h"

#pragma warning(disable: 4244)

namespace img {

void bilinearMap( 
	const PntArr2f &mapPlaces,  ///< wanted map coordinates
	PntArr2f &map,				///< interpolation object and output
	float reservedMapValue )	///< special value in map's x&y meaning absence of datum
{
    int width = map.Width();
    int height = mapPlaces.Height();
	assert( width==mapPlaces.Width() && height==mapPlaces.Height() );
	PntArr2f res( width, height );

	CFPUmode fpu_(downward);

	xtd::arr2d_wise_const_iterator<Point2f> place( mapPlaces, 0, 0 );
	xtd::arr2d_wise_iterator<Point2f> resIt( res, 0, 0 );
	for( ; place != mapPlaces.end(); ++place, ++resIt ) {
		int ux = QF2I(place->x);
		int uy = QF2I(place->y);
		float offsetX = place->x - ux;
		float offsetY = place->y - uy;

		float x1=0,x2=0,x3=0,x4=0;
		float y1=0,y2=0,y3=0,y4=0;

		bool valid1 = ( ux>=0 && uy>=0 && ux<width && uy<height ) && 
			(x1=map(ux,uy).x) != reservedMapValue && (y1=map(ux,uy).y) != reservedMapValue;
		bool valid2 = ( ux>=-1 && uy>=0 && ux<width-1 && uy<height ) && 
			(x2=map(ux+1,uy).x) != reservedMapValue && (y2=map(ux+1,uy).y) != reservedMapValue;
		bool valid3 = ( ux>=0 && uy>=-1 && ux<width && uy<height-1 ) && 
			(x3=map(ux,uy+1).x) != reservedMapValue && (y3=map(ux,uy+1).y) != reservedMapValue;
		bool valid4 = ( ux>=-1 && uy>=-1 && ux<width-1 && uy<height-1 ) && 
			(x4=map(ux+1,uy+1).x) != reservedMapValue && (y4=map(ux+1,uy+1).y) != reservedMapValue;

		float c1 = valid1 ? (1-offsetX)*(1-offsetY) : 0;
		float c2 = valid2 ? offsetX*(1-offsetY) : 0;
		float c3 = valid3 ? (1-offsetX)*offsetY : 0;
		float c4 = valid4 ? offsetX*offsetY : 0;

		float csum = c1+c2+c3+c4;
		if( csum < 0.75 ) {
			//not enough valid points
			resIt->x = resIt->y = reservedMapValue;
		} else {
			csum = 1. / csum;
			resIt->x = csum * (x1*c1 + x2*c2 + x3*c3 + x4*c4);
			resIt->y = csum * (y1*c1 + y2*c2 + y3*c3 + y4*c4);
		}
	}
	map.Swap(res);
}

void bilinearImage( 
	AImage &background,			///< result of interpolation overwrites pixels of this image
	bitmap &replace,			///< replaces pixels in background only if coresponding bit is set and immediatly resets the bit
	const PntArr2f &places,		///< wanted places in initial image
	const int_point &bDispl,	///< position of the top left corner of (background) image 'places' relative to 'places' origin
	const AImage &initial,		///< interpolation object = layer
	const int_point &iDispl,	///< position of the top left corner of layer relative to (0,0) in places data (or relative to image)
	const bRGB &noValue,		///< color from initial image that should not be written in background image
	bool interpolate,			///< if false finds bests existing color instead of interpolating
	bool noNoValue)				///< if true every initial color is background and transferred 
{
	assert(replace.width() == background.Width() && replace.height() == background.Height());
	assert(background.Width() <= places.Width() || background.Height() <= places.Height());

	int iwidth = initial.Width();
	int iheight = initial.Height();

	CFPUmode fpu_(downward);

	xtd::arr2d_wise_iterator<bRGB> backgroundIt( background, 0, 0 );
	for( ; backgroundIt != background.end(); ++backgroundIt ) {
		//(x,y) is a coordinate in output background image
		const int x = backgroundIt.x();
		const int y = backgroundIt.y();

		if (!replace(x,y)) 
			continue;

		//here (px, py) is a coordinate in preimage map
		const int px = x + bDispl.x;
		const int py = y + bDispl.y;

		//(lx,ly) & (ux,uy) are coordinates in undeformed layer
		float lx = places(px,py).x - iDispl.x;
		float ly = places(px,py).y - iDispl.y;

		int ux = QF2I(lx);
		int uy = QF2I(ly);
		float offsetX = lx - ux;
		assert (offsetX >= 0 && offsetX < 1);
		float offsetY = ly - uy;
		assert (offsetY >= 0 && offsetY < 1);

		bRGB x1(0,0,0),x2(0,0,0),x3(0,0,0),x4(0,0,0);

		bool valid1 = ( ux>=0 && uy>=0 && ux<iwidth && uy<iheight ) && 
			( x1=initial(ux,uy), noNoValue || x1 != noValue );
		bool valid2 = ( ux>=-1 && uy>=0 && ux<iwidth-1 && uy<iheight ) && 
			( x2=initial(ux+1,uy), noNoValue || x2 != noValue );
		bool valid3 = ( ux>=0 && uy>=-1 && ux<iwidth && uy<iheight-1 ) && 
			( x3=initial(ux,uy+1), noNoValue || x3 != noValue );
		bool valid4 = ( ux>=-1 && uy>=-1 && ux<iwidth-1 && uy<iheight-1 ) && 
			( x4=initial(ux+1,uy+1), noNoValue || x4 != noValue );

		float c1 = valid1 ? (1-offsetX)*(1-offsetY) : 0;
		float c2 = valid2 ? offsetX*(1-offsetY) : 0;
		float c3 = valid3 ? (1-offsetX)*offsetY : 0;
		float c4 = valid4 ? offsetX*offsetY : 0;

		float csum = c1+c2+c3+c4;
		if (csum < 0.75) 
			continue; //not enough valid points

		csum = 1. / csum;
		c1 *= csum;
		c2 *= csum;
		c3 *= csum;
		c4 *= csum;

		if( interpolate ) {
			backgroundIt->r = round( x1.r*c1 + x2.r*c2 + x3.r*c3 + x4.r*c4 );
			backgroundIt->g = round( x1.g*c1 + x2.g*c2 + x3.g*c3 + x4.g*c4 );
			backgroundIt->b = round( x1.b*c1 + x2.b*c2 + x3.b*c3 + x4.b*c4 );
			backgroundIt->a = round( x1.a*c1 + x2.a*c2 + x3.a*c3 + x4.a*c4 );
		} else {
			//accumulate data ab*backgroundIt nearest color
			if( valid1 && valid2 && x1==x2 ) c1+=c2;
			if( valid1 && valid3 && x1==x3 ) c1+=c3;
			if( valid1 && valid4 && x1==x4 ) c1+=c4;

			if( valid2 && valid3 && x2==x3 ) c2+=c3;
			if( valid2 && valid4 && x2==x4 ) c2+=c4;

			if( valid3 && valid4 && x3==x4 ) c3+=c4;

			//find max coefficient and store appropriate color as result
			if( c1 > c2 ) {
				if( c1 > c3 ) {
					if( c1 > c4 ) {
						*backgroundIt = x1;
					} else {
						*backgroundIt = x4;
					}
				} else {
					if( c3 > c4 ) {
						*backgroundIt = x3;
					} else {
						*backgroundIt = x4;
					}
				}
			} else {
				if( c2 > c3 ) {
					if( c2 > c4 ) {
						*backgroundIt = x2;
					} else {
						*backgroundIt = x4;
					}
				} else {
					if( c3 > c4 ) {
						*backgroundIt = x3;
					} else {
						*backgroundIt = x4;
					}
				}
			}
		}
		replace(x,y) = false;
	}
}

}// namespace img
